#ifndef LOCALIZATION_DYNAMICS_MODEL_H_
#define LOCALIZATION_DYNAMICS_MODEL_H_

#include <gsl/gsl_blas.h>
#include <libplayerc++/playerc++.h>
#include <core/macros.h>

class DynamicsModel {
public:
  DynamicsModel(PlayerCc::Position2dProxy *position2d_proxy);
  ~DynamicsModel();

  void UpdateBelief(player_pose2d *mean, gsl_matrix *covariance);
private:
  // Empirical uncertainty constants
  static const double kCovarTransTrans = 0.033;
  static const double kCovarThetaTheta = 0.035;
  static const double kCovarTransTheta = 0.001;

  // Matrices used in the calculations
  gsl_matrix * G_;
  gsl_matrix * V_;
  gsl_matrix * M_;
  gsl_matrix * aux_3x3_;
  gsl_matrix * aux_3x2_;

  // Last reading obtained from the robot's odometer
  player_pose2d last_reading_;

  // Proxy used to access the robot's odometry data
  PlayerCc::Position2dProxy *position2d_proxy_;

  DISALLOW_IMPLICIT_CONSTRUCTORS(DynamicsModel);
};

#endif
